#include "SC_PlugIn.h"

// InterfaceTable contains pointers to functions in the host (server).
static InterfaceTable *ft;

//Nick Collin's WaveTerrain = WaveTerrain(bufnum, x, y, xsize, ysize, wrapbehaviour)
struct HyperTerrain : public Unit
{
        int m_size, m_xsize, m_ysize;
        int* dimSizes;
        int numDimensions;
        float * m_terrain;
        float * orbit;
        bool cubicInterpolation;
};

extern "C" {
    void load(InterfaceTable *inTable);
    void HyperTerrain_next_a(HyperTerrain *unit, int inNumSamples);
    void HyperTerrain_Ctor(HyperTerrain* unit);
    SndBuf * SLUGensGetBuffer(Unit * unit, uint32 bufnum);
    double interpolateL(double mu,double a0,double a1);
    double CubicInterpolate(
        double y0,double y1,
        double y2,double y3,
        double mu);
}

void HyperTerrain_Ctor(HyperTerrain* unit) {
    //World *world = unit->mWorld;

    uint32 bufnum = (uint32)ZIN0(0);
    SndBuf * buf= SLUGensGetBuffer(unit,bufnum);
    unit->cubicInterpolation = false;
    if (buf) {
        //if (bufnum >= world->mNumSndBufs) bufnum = 0;
        //unit->m_bufnum=bufnum;
        //SndBuf *buf = world->mSndBufs + bufnum;

        unit->m_size = buf->samples;
        unit->numDimensions = (int)(ZIN0(1));
        unit->dimSizes = new int[unit->numDimensions];
        for(int i=0;i<unit->numDimensions;i++) {
            unit->dimSizes[i] = (int)(ZIN0(2+i)+0.0001);//safety on rounddown
        }
        int size=1;
        for(int i=0;i<unit->numDimensions;i++) {
            printf("%d\n", unit->dimSizes[i]);
            size = size*unit->dimSizes[i];
        }

        if(size != unit->m_size) {

            printf("WaveTerrain: size mismatch between dimension sizes and actual buffer size. UGen will output silence \n");
            printf("%d\n",unit->m_size);
            printf("%d\n",size);
            SETCALC(*ClearUnitOutputs);
            return;
        } else {
            printf("%d\n",size);
        }

        unit->m_terrain= buf->data;

        SETCALC(HyperTerrain_next_a);
    }

}

void HyperTerrain_next_a(HyperTerrain *unit, int inNumSamples) {

    float * terrain = unit->m_terrain;
    //int size= unit->m_size;

    float *out = ZOUT(0);
    /*
    float x, y, xindex, yindex;
    int xfloor, yfloor, xnext, ynext;
    float xprop,yprop;
    */
    //float vll,vlr,vul,vur;


    float orbitValues[unit->numDimensions];
    float indices[unit->numDimensions];
    int floors[unit->numDimensions];
    int nexts[unit->numDimensions];
    float mus[unit->numDimensions];
    int vertices[(int)pow(2,unit->numDimensions)];
    float interpolations[unit->numDimensions][(unit->numDimensions-1)*2];
    int floorIndices[unit->numDimensions];
    int nextIndices[unit->numDimensions];
    int subfloors[unit->numDimensions];
    int supernexts[unit->numDimensions];
    int subfloorIndices[unit->numDimensions];
    int supernextIndices[unit->numDimensions];
    int cubicvertices[(int)pow(4,unit->numDimensions)];
    float cubicinterpolations[unit->numDimensions][(unit->numDimensions-1)*8];

    for(int j=0; j<inNumSamples;++j) {
        for(int d=0; d<unit->numDimensions;d++) {
            orbitValues[d] = IN(3+d+unit->numDimensions)[j];
            //printf("orbit dimension %i = %f\n",d,orbitValues[d]);

            //safety
            orbitValues[d] = sc_wrap(orbitValues[d], 0.0f, 1.f);

            indices[d] = orbitValues[d]*unit->dimSizes[d];

            //printf("dim size %i = %i \n", d,unit->dimSizes[d]);
           // printf("indices %i = %f \n",d,indices[d]);

            //these are guaranteed in range from wrap above give or take floating point error on round down?
            //added modulo because very occasional error with index up to xsize or ysize
            floors[d] = ((int)indices[d])%unit->dimSizes[d];

            //these aren't; needs further wrap
            nexts[d] = (floors[d]+1)%unit->dimSizes[d];

            //proportion between value and floor
            mus[d] = indices[d]-floors[d];
            //printf("muortion %i = %f\n",d,mus[d]);

            //For Cubic Interpolation:
            if(unit->cubicInterpolation) {
                //subfloors. Wrap for safety
                subfloors[d] =  (floors[d] - 1)%unit->dimSizes[d];

                //supernexts. Wrap for Safety
                supernexts[d] =  (nexts[d] + 1)%unit->dimSizes[d];
            }
        }
        //now have to look up in table and interpolate; linear within the numDimensions**2 vertices of a cell for now, cubic over numDimensions**8 vertices maybe later
        /*
            example of how this scales over various dimensions(using only floors):
            1 = xfloor;
            2 = (xSize*yfloor) + xfloor;
            3 = (xSize*ySize*zFloor) + (xSize*yfloor) + xfloor;
            4 = (xSize*ySize*zSize*qfloor) + (xSize*ySize*zFloor) + (xSize*yfloor) + xfloor;
          */
            if(!unit->cubicInterpolation) {
                for(int v=0;v<(int)pow(2,unit->numDimensions);v++) {
                    vertices[v] = 0;
                }
                //Calculate floor and next indices;
                for(int d=0; d<unit->numDimensions;d++) {
                    int floorI = 1;
                    int nextI = 1;
                    for(int i=0;i<d;i++) {
                        floorI*= unit->dimSizes[i];
                        nextI*= unit->dimSizes[i];
                    }
                    floorI *= floors[d];
                    nextI *= nexts[d];
                    floorIndices[d] = floorI;
                    nextIndices[d] = nextI;

                    //Create all combinatorial iterations of nexts and floors for each dimension to create vertices
                    int combo = 0;
                    //floor or next?
                    bool forn = true;
                    for(int i=0; i<(int)pow(2,unit->numDimensions);i++) {
                        if(combo==0) {
                            forn = !forn;
                        }
                        if(forn) {
                            vertices[i] += nexts[d];
                            //printf("dimension %i, vertice %i, next \n",d,i);
                        } else {
                            vertices[i] += floors[d];
                            //printf("dimension %i, vertice %i, floor \n",d,i);
                        }
                        combo = (combo+1)%((int)pow(2,d));
                    }
                }

                //Interpolate between vertices
                for(int d=unit->numDimensions-1;d>=0;d-=1) {
                    for(int i=0;i<(int)pow(2,d+1);i+=2) {
                        //printf("interpolating dimension %i,mu %i, a0 %i,a1 %1",d,mus[(unit->numDimensions-1)-d],terrain[vertices[i]],terrain[vertices[i+1]]);
                        //printf("interpolating dimension %i,mu %f, a0 %f,a1 %f \n",d,mus[(unit->numDimensions-1)-d],terrain[vertices[i]],terrain[vertices[i+1]]);
                        if(d==unit->numDimensions-1) {
                            interpolations[d][(int)(i*0.5)] = interpolateL(mus[d],terrain[vertices[i]],terrain[vertices[i+1]]);
                        } else {
                            interpolations[d][(int)(i*0.5)] = interpolateL(mus[d],interpolations[d+1][i],interpolations[d+1][i+1]);
                        }
                    }
                }
                //Final Output after interpolation
                ZXP(out) = interpolations[0][0];
            } else {
                for(int v=0;v<(int)pow(4,unit->numDimensions);v++) {
                    cubicvertices[v] = 0;
                }
                    //Calculate floor and next indices;
                    for(int d=0; d<unit->numDimensions;d++) {
                        int floorI = 1;
                        int nextI = 1;
                        int subfloorI = 1;
                        int supernextI = 1;

                        for(int i=0;i<d;i++) {
                            floorI*= unit->dimSizes[i];
                            nextI*= unit->dimSizes[i];
                            subfloorI*= unit->dimSizes[i];
                            supernextI*= unit->dimSizes[i];
                        }

                        floorI *= floors[d];
                        nextI *= nexts[d];
                        subfloorI *= subfloors[d];
                        supernextI *= supernexts[d];

                        floorIndices[d] = floorI;
                        nextIndices[d] = nextI;
                        subfloorIndices[d] = subfloorI;
                        supernextIndices[d] = supernextI;

                        //Create all combinatorial iterations of nexts and floors for each dimension to create vertices
                        int combo = 0;
                        //floor or next?
                        int sffnorsn = 0;
                        for(int i=0; i<(int)pow(4,unit->numDimensions);i++) {
                            /*
                            if(forn) {
                                vertices[i] += nexts[d];
                                //printf("dimension %i, vertice %i, next \n",d,i);
                            } else {
                                vertices[i] += floors[d];
                                //printf("dimension %i, vertice %i, floor \n",d,i);
                            }
                            */

                            switch(sffnorsn) {
                                case 0:
                                    cubicvertices[i] += subfloors[d];
                                    //printf("dimension %i, vertice %i, subfloor \n",d,i);
                                    break;
                                case 1:
                                    cubicvertices[i] += floors[d];
                                    //printf("dimension %i, vertice %i, floor \n",d,i);
                                    break;
                                case 2:
                                    cubicvertices[i] += nexts[d];
                                    //printf("dimension %i, vertice %i, next \n",d,i);
                                    break;
                                case 3:
                                    cubicvertices[i] += supernexts[d];
                                    //printf("dimension %i, vertice %i, supernext \n",d,i);
                                    break;
                            }

                            combo = (combo+1)%((int)pow(4,d));
                            if(combo==0) {
                                sffnorsn = (sffnorsn+1)%4;
                            }
                        }
                    }

                    //Interpolate between vertices
                    for(int d=unit->numDimensions-1;d>=0;d-=1) {
                        for(int i=0;i<(int)pow(4,d+1);i+=4) {
                            //printf("interpolating dimension %i,mu %i, a0 %i,a1 %1",d,mus[(unit->numDimensions-1)-d],terrain[vertices[i]],terrain[vertices[i+1]]);
                            //printf("interpolating dimension %i,mu %f, a0 %f,a1 %f \n",d,mus[(unit->numDimensions-1)-d],terrain[vertices[i]],terrain[vertices[i+1]]);
                            if(d==unit->numDimensions-1) {
                                cubicinterpolations[d][(int)(i*0.25)] = CubicInterpolate(mus[d],terrain[cubicvertices[i]],terrain[cubicvertices[i+1]],terrain[cubicvertices[i+2]],terrain[cubicvertices[i+3]]);
                            } else {
                                cubicinterpolations[d][(int)(i*0.25)] = CubicInterpolate(mus[d],interpolations[d+1][i],interpolations[d+1][i+1],interpolations[d+1][i+2],interpolations[d+1][i+3]);
                            }
                        }
                    }

                    //Final Output after interpolation
                    ZXP(out) = cubicinterpolations[0][0];
            }
    }

    /*
    for (int j=0; j<inNumSamples;++j) {

        x= xin[j]; //0.0 to 1.0
        y= yin[j];

        //safety
        x= sc_wrap(x, 0.0f, 1.f);
        y= sc_wrap(y, 0.0f, 1.f);

        xindex= x*xsize;
        yindex= y*ysize;

        //these are guaranteed in range from wrap above give or take floating point error on round down?
        //added modulo because very occasional error with index up to xsize or ysize
        xfloor= ((int)xindex)%xsize;
        yfloor= ((int)yindex)%ysize;

        //these aren't; needs further wrap
        xnext= (xfloor+1)%xsize;
        ynext= (yfloor+1)%ysize;

        xmu= xindex-xfloor;
        ymu= yindex-yfloor;

        //printf("x %f, y %f, xfloor %d, yfloor %d, xnext, %d, ynext %d, xprop %f, yprop %f \n", x, y, xfloor, yfloor, xnext, ynext, xprop, yprop);

        //now have to look up in table and interpolate; linear within the 4 vertices of a square cell for now, cubic over 16 vertices maybe later

        //format for terrain should be rows of xsize, indexed from lower left
        vll= terrain[(xsize*yfloor)+ xfloor];
        vlr= terrain[(xsize*yfloor)+ xnext];
        vul= terrain[(xsize*ynext)+ xfloor];
        vur= terrain[(xsize*ynext)+ xnext];

        ZXP(out) = (1.0-xprop)*(vll+(yprop*(vul-vll))) + (xprop*(vlr+(yprop*(vur-vlr))));

        //printf("%f \n",);
    }
    */

}

double interpolateL(double mu,double a0,double a1)
{
    return (mu * a0) + ((1-mu) * a1);
}

double CubicInterpolate(
   double y0,double y1,
   double y2,double y3,
   double mu)
{
   double a0,a1,a2,a3,mu2;

   mu2 = mu*mu;
   a0 = y3 - y2 - y0 + y1;
   a1 = y0 - y1 - a0;
   a2 = y2 - y0;
   a3 = y1;

   return(a0*mu*mu2+a1*mu2+a2*mu+a3);
}

//include local buffer test in one place
SndBuf * SLUGensGetBuffer(Unit * unit, uint32 bufnum) {

        SndBuf *buf;
        World *world = unit->mWorld;

        if (bufnum >= world->mNumSndBufs) {
                int localBufNum = bufnum - world->mNumSndBufs;
                Graph *parent = unit->mParent;
                if(localBufNum <= parent->localMaxBufNum) {
                        buf = parent->mLocalSndBufs + localBufNum;
                } else {
                        if(unit->mWorld->mVerbosity > -1){ Print("SLUGens buffer number error: invalid buffer number: %i.\n", bufnum); }
                        SETCALC(*ClearUnitOutputs);
                        unit->mDone = true;
                        return NULL;
                }
        } else {
                buf = world->mSndBufs + bufnum;
        }

        return buf;
}

////////////////////////////////////////////////////////////////////
void load(InterfaceTable *inTable)
{
        ft = inTable;
        DefineSimpleUnit(HyperTerrain);
}
